#include "perception_ros2_driver/node.hpp"

#include <functional>
#include <string>
#include <utility>
#include <vector>

double getCurrentTime() {
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return static_cast<double>(tv.tv_sec * 1000.0 + tv.tv_usec / 1000.0);
}

struct PointXYZIR
{
  float X{0.0F};
  float Y{0.0F};
  float Z{0.0F};
  uint8_t Intensity{0};
  uint8_t Ring{0};
};

POINT_CLOUD_REGISTER_POINT_STRUCT(
  PointXYZIR,
  (float, X, X)(float, Y, Y)(float, Z, Z)(uint8_t, Intensity, Intensity)(uint8_t, Ring, Ring))

PerceptionDriverNode::PerceptionDriverNode()
: Node("perception_driver")
{
  pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lq_lidar_pointcloud", rclcpp::QoS{1});
  target_pub_ = this->create_publisher<perception_ros2_driver::msg::Targets>("/lq_lidar_targets", rclcpp::QoS{1});
  status_pub_ = this->create_publisher<perception_ros2_driver::msg::Status>("/lq_lidar_status", rclcpp::QoS{1});

  const auto period_ns = rclcpp::Rate(10.0).period();
  status_timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&PerceptionDriverNode::runStatusReport, this));

  lidar_ip_ = this->declare_parameter<std::string>("lidar_ip", "");
  http_client_config config;
  config.set_timeout(std::chrono::seconds(1));
  client_.reset(new http_client(std::string("http://") + lidar_ip_, config));
  recv_thread_.reset(new std::thread(&PerceptionDriverNode::runRecvLoop, this));
}

void PerceptionDriverNode::runRecvLoop()
{
  while(true) {
    web::json::value data;
    data["display_on_image"] = web::json::value::boolean(false);
    data["sample_size"] = 0;
    std::vector<unsigned char> result;
    try {
      auto response = client_->request(methods::POST, U("/v1/detection-pb"), data).get();
      result = response.extract_vector().get();
    } catch (const http_exception& e) {
      std::cout << "error:" << e.error_code().message() << std::endl;
      usleep(100000);
      continue;
    }

    Detection det;
    if (!det.ParseFromArray(result.data(), result.size())) {
      continue;
    }
    if (!det.has_header() || !det.header().has_timestamp() || !det.has_points()) {
      continue;
    }

    pcl::PointCloud<PointXYZIR>::Ptr pointcloud(new pcl::PointCloud<PointXYZIR>);
    std::string* points_raw = det.mutable_points();
    size_t points_num = points_raw->size() / 4 / 4;
    float *points = (float *)(points_raw->data());
    for (size_t i = 0; i < points_num; i = i + 4) {
      PointXYZIR point;
      point.X = points[i + 0];
      point.Y = points[i + 1];
      point.Z = points[i + 2];
      point.Intensity = uint8_t(points[i + 3]);
      point.Ring = uint8_t(i % 128);
      pointcloud->push_back(point);
    }

    sensor_msgs::msg::PointCloud2 pointcloud_msg;
    pcl::toROSMsg(*pointcloud, pointcloud_msg);
    pointcloud_msg.header.frame_id = "base_link";
    pointcloud_msg.header.stamp = this->now();
    pointcloud_pub_->publish(pointcloud_msg);

    perception_ros2_driver::msg::Targets targets_msg;
    targets_msg.us_target_num = det.object_size();
    for (auto object : det.object()) {
      perception_ros2_driver::msg::Target target;
      target.us_target_id = object.id();
      target.uc_target_conf = uint8_t(object.confidence() * 100);
      target.i_target_x = object.box().center().x() * 100;
      target.i_target_y = object.box().center().y() * 100;
      target.i_target_z = object.box().center().z() * 100;
      target.us_target_length = object.box().length() * 100;
      target.us_target_width = object.box().width() * 100;
      target.us_target_height = object.box().height() * 100;
      target.ui_target_direct = object.box().heading() / M_PI * 180 * 100;
      target.s_target_vx = object.velocity_x() * 100;
      target.s_target_vy = object.velocity_y() * 100;
      targets_msg.targets.emplace_back(target);
    }
    target_pub_->publish(targets_msg);

    std::cout << "publish points: " << points_num << ", target: " << det.object_size() << std::endl;
    usleep(10000);
  }
}

void PerceptionDriverNode::runStatusReport()
{
  web::json::value data;
  data["host_date"] = getCurrentTime();
  perception_ros2_driver::msg::Status status_msg;
  try {
    auto response = client_->request(methods::POST, U("/v1/status"), data).get();
    web::json::value status = response.extract_json().get();
    web::json::object lidars = status.at("lidar").as_object();
    bool valid = lidars["0-Falcon-Prime"]["valid"].as_bool();
    status_msg.uc_radar_status = valid ? 0 : 2;
  } catch (const http_exception& e) {
    status_msg.uc_radar_status = 1;
  }
  status_pub_->publish(status_msg);
  std::cout << "radar status: " << int(status_msg.uc_radar_status) << std::endl;
}